/**
 ******************************************************************************
 * @file    icm20948.c
 * @author  Hsiang Hsu, liwumi
 * @version V1.0.0
 * @date    2025-05-16
 * @brief   ICM20948 驱动程序
 ******************************************************************************
 **/

/* Includes ------------------------------------------------------------------*/
#include "icm20948.h"

/* Private typedef -----------------------------------------------------------*/
// ICM20948 错误代码枚举
typedef enum
{
  ICM_OK = 0,
  ICM_WAKE_UP_FAILED,
  ICM_SELF_TEST_FAILED,
  ICM_FIFO_CONFIG_FAILED,
  ICM_INT_CONFIG_FAILED,
  ICM_ACC_CONFIG_FAILED,
  ICM_GYRO_CONFIG_FAILED
} ERROR_CODE;
// ICM20948 中断状态枚举
typedef enum
{
  OTHERS_INT,
  RAW_DATA_0_RDY_INT,
  FIFO_OVERFLOW_INT,
  FIFO_WM_INT
} ICM_INT_STATUS;
// ICM20948 原始数据结构体
typedef struct
{
  uint8_t icm_id;
  uint8_t acc_xh;
  uint8_t acc_xl;
  uint8_t acc_yh;
  uint8_t acc_yl;
  uint8_t acc_zh;
  uint8_t acc_zl;
  uint8_t gyro_xh;
  uint8_t gyro_xl;
  uint8_t gyro_yh;
  uint8_t gyro_yl;
  uint8_t gyro_zh;
  uint8_t gyro_zl;
} ICMOriginalDataTypeDef;
/* Private define ------------------------------------------------------------*/
// 操作 SDI
#define SPI2_MOSI_OUT(n) (n ? LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_5) \
                            : LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_5))
/* Private macro -------------------------------------------------------------*/
// 【TODO】：将所有寄存器地址替换为宏定义
/* Private variables ---------------------------------------------------------*/
static uint32_t icm_frame_count = 0; // 帧计数器
/* Private function prototypes -----------------------------------------------*/

static void ICMErrorHandler(ERROR_CODE error_code);           // ICM20948 内部错误处理函数
static uint8_t ICMSelfTest(void);                             // ICM20948 自检函数
static uint8_t ICMFIFOConfig(void);                           // ICM20948 FIFO 配置函数
static uint8_t ICMINTConfig(void);                            // ICM20948 中断配置函数
static uint8_t ICMAccConfig(void);                            // ICM20948 加速度计配置函数
static uint8_t ICMGyroConfig(void);                           // ICM20948 陀螺仪配置函数
static void ICMGetOriginalData(ICMOriginalDataTypeDef *pbuf); // ICM20948 原始数据获取函数
/* Private functions ---------------------------------------------------------*/

/*******************************************************************************
** 名    称 ： ICMErrorHandler
** 功    能 ： icm20948.c 内部错误处理函数，如果出现错误，将控制小灯周期性闪烁。
** 注    意 ： 参考 ERROR_CODE 诊断错误原因，闪烁次数 = 代码值
** 入口参数 ： error_code: 错误代码
** 出口参数 ： 无
*******************************************************************************/
static void ICMErrorHandler(ERROR_CODE error_code)
{
  // 【TODO】：设计更加智能的错误提示方案
  LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_0); // 熄灭 LED1
  while (1)
  {
    for (int i = 1; i <= error_code; i++)
    {
      LED1_Toggle;
      LL_mDelay(100 * i);
      LED1_Toggle;
      LL_mDelay(100 * i);
    }
  }
}

/*******************************************************************************
** 名    称 ： ICMSelfTest
** 功    能 ： ICM20948 自检函数，目前不支持磁力计
** 入口参数 ： 无
** 出口参数 ： uint8_t flag: 自检结果(0: 失败，1: 成功， 8 位依次对应 acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, l, h)
*******************************************************************************/
static uint8_t ICMSelfTest(void)
{
  uint8_t flag = 0;
  uint8_t rx_buf = 0x00;
  // 【TODO】：完成 ICM20948 自检
  /* 陀螺仪自检 */
  // 1. 配置寄存器(关闭自检)
  // 2. 读取陀螺仪数据
  // 3. 配置寄存器(开启自检)
  // 4. 读取陀螺仪数据
  // 5. 读取 SELF RESPONSE
  // 6. 自检结果判断
  /*【TODO】加速度计自检 */
  /*【TODO】磁力计自检 */
  return flag;
}

/*******************************************************************************
** 名    称 ： ICMFIFOConfig
** 功    能 ： ICM20948 FIFO 配置函数
** 入口参数 ： 无
** 出口参数 ： uint8_t flag: 配置结果(0: 成功， 1: 失败)
*******************************************************************************/
static uint8_t ICMFIFOConfig(void)
{
  uint8_t flag = 0;
  uint8_t rx_buf = 0x00;
  // 配置 USER_CTRL 0x40: 0100 0000
  SwapByteSPI2(0x40, 0x03, 0, &rx_buf); // Enable FIFO operation
  SwapByteSPI2(0xFF, 0x03, 1, &rx_buf);
  flag |= (rx_buf != 0x40);
  // 配置 FIFO_EN_1 0x00 (default)
  // 配置 FIFO_EN_2 0x1E: 0001 1110
  SwapByteSPI2(0x10, 0x67, 0, &rx_buf); // Enable ACCLE_FIFO and GYRO_?_FIFO
  SwapByteSPI2(0xFF, 0x67, 1, &rx_buf);
  flag |= (rx_buf != 0x10) << 1;
  // 配置 FIFO_RST 0x1F: 0001 1111
  SwapByteSPI2(0x1F, 0X68, 0, &rx_buf); // Reset FIFO size to 0
  SwapByteSPI2(0xFF, 0x68, 1, &rx_buf);
  flag |= (rx_buf != 0x1F);
  // 配置 FIFO_MODE 0x00 (default)
  // 配置 FIFO_CFG 0x01: 0000 0001
  SwapByteSPI2(0x01, 0x76, 0, &rx_buf); // interrupt status for each sensor
  SwapByteSPI2(0xFF, 0x76, 1, &rx_buf);
  flag |= (rx_buf != 0x01) << 4;
  return flag;
}

/*******************************************************************************
** 名    称 ： ICMINTConfig
** 功    能 ： ICM20948 中断配置函数
** 入口参数 ： 无
** 出口参数 ： uint8_t flag: 配置结果(0: 成功， 1: 失败)
*******************************************************************************/
static uint8_t ICMINTConfig(void)
{
  // 【TODO】：将所有无需配置的寄存器通信代码注释掉
  uint8_t flag = 0;
  uint8_t rx_buf = 0x00;
  // 配置 INT_PIN_CFG  0x0 (默认，可不配置)
  SwapByteSPI2(0x00, 0x0F, 0, &rx_buf); // 注意：这里配置的是读取 STATUS 清除标志位
  SwapByteSPI2(0xFF, 0x0F, 1, &rx_buf);
  flag |= (rx_buf != 0x00);
  // 配置 INT_ENABLE 0x00 (默认，可不配置)
  SwapByteSPI2(0x00, 0x10, 0, &rx_buf);
  SwapByteSPI2(0xFF, 0x10, 1, &rx_buf);
  flag |= (rx_buf != 0x00);
  // 配置 INT_ENABLE_1 0x01 (RAW_DATA_0_RDY_EN)
  SwapByteSPI2(0x01, 0x11, 0, &rx_buf);
  SwapByteSPI2(0xFF, 0x11, 1, &rx_buf);
  flag |= (rx_buf != 0x01);
  // 配置 INT_ENABLE_2 0x01 (FIFO_OVERFLOW_EN)
  SwapByteSPI2(0x00, 0x12, 0, &rx_buf);
  SwapByteSPI2(0xFF, 0x12, 1, &rx_buf);
  flag |= (rx_buf != 0x00) << 1;
  // 配置 INT_ENABLE_3 0x01 (FIFO_WM_EN)
  SwapByteSPI2(0x00, 0x13, 0, &rx_buf);
  SwapByteSPI2(0xFF, 0x13, 1, &rx_buf);
  flag |= (rx_buf != 0x00) << 2;
  return flag;
}

/*******************************************************************************
** 名    称 ： ICMAccConfig
** 功    能 ： ICM20948 加速度计配置函数
** 入口参数 ： 无
** 出口参数 ： uint8_t flag: 配置结果(0: 成功， 1: 失败)
*******************************************************************************/
static uint8_t ICMAccConfig(void)
{
  uint8_t flag = 0;
  uint8_t rx_buf = 0x00;
  // 配置 REG_BANK_SEL 选择 Bank 2
  SwapByteSPI2(0x20, 0x7F, 0, &rx_buf);
  SwapByteSPI2(0xFF, 0x7F, 1, &rx_buf);
  flag |= (rx_buf != 0x20);
  // 配置 ACCEL_SMPLRT_DIV_1
  SwapByteSPI2(0x00, 0x10, 0, &rx_buf);
  SwapByteSPI2(0xFF, 0x10, 1, &rx_buf);
  flag |= (rx_buf != 0x00);
  // 配置 ACCEL_SMPLRT_DIV_2
  SwapByteSPI2(0x01, 0x11, 0, &rx_buf);
  SwapByteSPI2(0xFF, 0x11, 1, &rx_buf);
  flag |= (rx_buf != 0x01);
  // 配置 ACCEL_CONFIG 0x01
  SwapByteSPI2(0x01, 0x14, 0, &rx_buf);
  SwapByteSPI2(0xFF, 0x14, 1, &rx_buf);
  flag |= (rx_buf != 0x01);
  // 配置 ACCEL_CONFIG_2
  // 配置 ACCEL_INTEL_CTRL
  // REG_BANK_SEL 选择 Bank 0
  SwapByteSPI2(0x00, 0x7F, 0, &rx_buf);
  SwapByteSPI2(0xFF, 0x7F, 1, &rx_buf);
  flag |= (rx_buf != 0x00);
  return flag;
}

/*******************************************************************************
** 名    称 ： ICMGyroConfig
** 功    能 ： ICM20948 加速度计配置函数
** 入口参数 ： 无
** 出口参数 ： uint8_t flag: 配置结果(0: 成功， 1: 失败)
*******************************************************************************/
static uint8_t ICMGyroConfig(void)
{
  uint8_t flag = 0;
  uint8_t rx_buf = 0x00;
  // 配置 REG_BANK_SEL 选择 Bank 2
  SwapByteSPI2(0x20, 0x7F, 0, &rx_buf);
  SwapByteSPI2(0xFF, 0x7F, 1, &rx_buf);
  flag |= (rx_buf != 0x20);
  // 配置 GYRO_SMPLRT_DIV
  SwapByteSPI2(0x01, 0x00, 0, &rx_buf);
  SwapByteSPI2(0xFF, 0x00, 1, &rx_buf);
  flag |= (rx_buf != 0x01);
  // 配置 GYRO_CONFIG_1
  // 配置 GYRO_CONFIG_2
  // REG_BANK_SEL 选择 Bank 0
  SwapByteSPI2(0x00, 0x7F, 0, &rx_buf);
  SwapByteSPI2(0xFF, 0x7F, 1, &rx_buf);
  flag |= (rx_buf != 0x00);
  return flag;
}

// 【TODO】：编写 ICM20948 Reset 函数
//  注意 STM32 复位不会导致外设复位

/*******************************************************************************
** 名    称 ： ICMGetInterruptStatus
** 功    能 ： ICM20948 中断状态读取函数
** 入口参数 ： ICMIntStatusTypeDef *pbuf: 存放 ICM20948 中断状态的结构体指针
** 出口参数 ： 无
*******************************************************************************/
// 【BUG】：还未完成
void ICMGetInterruptStatus(ICMInterruptStatusTypeDef *pbuf)
{
  // 【TODO】：当下受测试要求，暂时设为 PUBLIC 函数，后期改为 PRIVATE 函数
  //  读取 INT_STATUS (无需读取)
  SwapByteSPI2(0xFF, 0x19, 1, &pbuf->others);
  // 读取 INT_STATUS_1 (Sensor Register Raw Data Update interrupt)
  SwapByteSPI2(0xFF, 0x1A, 1, &pbuf->raw_data_0_rdy);
  // 读取 INT_STATUS_2 (FIFO Overflow interrupt)
  SwapByteSPI2(0xFF, 0x1B, 1, &pbuf->fifo_overflow);
  // 读取 INT_STATUS_3 (FIFO Watermark interrupt)
  SwapByteSPI2(0xFF, 0x1C, 1, &pbuf->fifo_watermark);
}

/*******************************************************************************
** 名    称 ： ICMGetFIFOData
** 功    能 ： ICM20948 FIFO 数据读取函数
** 入口参数 ： uint8_t *pdata: 存放 FIFO 数据的指针
** 出口参数 ： FIFO 中数据个数 FIFO_COUNT
*******************************************************************************/
// 【BUG】：还未完成
uint16_t ICMGetFIFOData(uint8_t *pdata)
{
  // 【TODO】：当下受测试要求，暂时设为 PUBLIC 函数，后期改为 PRIVATE 函数
  uint8_t count[2] = {0};
  // 读取 FIFO_COUNTH, FIFO_COUNTL
  SwapByteSPI2(0xFF, 0x70, 1, count + 1);
  SwapByteSPI2(0xFF, 0x71, 1, count);
  // 【TODO】读取 DATA_RDY_STATUS，不知道有什么用
  SwapByteSPI2(0xFF, 0x74, 1, pdata);
  // 读取 FIFO_R_W
  SwapByteSPI2(0xFF, 0x72, 1, pdata);
  return (count[1] << 8) | count[0];
}

/*******************************************************************************
** 名    称 ： ICMGetOriginalData
** 功    能 ： 获取 ICM20948 原始数据(m/s^2, rad/s, uT)
** 入口参数 ： pbuf: 存放 ICM20948 数据的结构体指针
** 出口参数 ： 无
*******************************************************************************/
static void ICMGetOriginalData(ICMOriginalDataTypeDef *pbuf)
{
  // 读取 ICM20948 数据
  // 1. 读取加速度计数据
  SwapByteSPI2(0xFF, 0x2D, 1, &pbuf->acc_xh);
  SwapByteSPI2(0xFF, 0x2E, 1, &pbuf->acc_xl);
  SwapByteSPI2(0xFF, 0x2F, 1, &pbuf->acc_yh);
  SwapByteSPI2(0xFF, 0x30, 1, &pbuf->acc_yl);
  SwapByteSPI2(0xFF, 0x31, 1, &pbuf->acc_zh);
  SwapByteSPI2(0xFF, 0x32, 1, &pbuf->acc_zl);
  // 2. 读取陀螺仪数据
  SwapByteSPI2(0xFF, 0x33, 1, &pbuf->gyro_xh);
  SwapByteSPI2(0xFF, 0x34, 1, &pbuf->gyro_xl);
  SwapByteSPI2(0xFF, 0x35, 1, &pbuf->gyro_yh);
  SwapByteSPI2(0xFF, 0x36, 1, &pbuf->gyro_yl);
  SwapByteSPI2(0xFF, 0x37, 1, &pbuf->gyro_zh);
  SwapByteSPI2(0xFF, 0x38, 1, &pbuf->gyro_zl);
  // 【TODO】 3. 读取磁力计数据
  // 【TODO】 4. 错误处理
}

/* Public functions ----------------------------------------------------------*/

/*******************************************************************************
** 名    称 ： ICM_Init
** 功    能 ： ICM20948 初始化(唤醒 ICM20948，自测)
** 入口参数 ： 无
** 出口参数 ： 无
*******************************************************************************/
void ICM_Init(void)
{
  // 【TODO】：完成 ICM20948 初始化
  uint8_t rx_buf = 0x00;
  // 1. 唤醒 ICM20948，禁用 temperature 传感器
  SwapByteSPI2(0x05, 0x06, 0, &rx_buf);
  SwapByteSPI2(0x05, 0x06, 0, &rx_buf); // 重复 2 次，确保 ICM20948 唤醒成功
  // 2. 配置检验
  SwapByteSPI2(0xFF, 0x06, 1, &rx_buf);
  SwapByteSPI2(0xFF, 0x06, 1, &rx_buf); // 重复 2 次
  if (rx_buf != 0x05)
  {
    ICMErrorHandler(ICM_WAKE_UP_FAILED);
  }
  // 5. FIFO 配置
  if (ICMFIFOConfig())
  {
    ICMErrorHandler(ICM_FIFO_CONFIG_FAILED);
  }
  // 6. 中断配置
  if (ICMINTConfig())
  {
    ICMErrorHandler(ICM_INT_CONFIG_FAILED);
  }
  // 7. 加速度计配置
  if (ICMAccConfig())
  {
    ICMErrorHandler(ICM_ACC_CONFIG_FAILED);
  }
  // 8. 陀螺仪配置
  if (ICMGyroConfig())
  {
    ICMErrorHandler(ICM_GYRO_CONFIG_FAILED);
  }
  // 9. 磁力计配置
  // 10. 帧同步配置
  // 11. 进行自检
  // if (ICMSelfTest())
  // {
  //   ICMErrorHandler(ICM_SELF_TEST_FAILED);
  // }
}

/*******************************************************************************
** 名    称 : SwapByteSPI2
** 功    能 : SPI2 数据交换(只交换 1 Byte)，在本模块上，SPI2 用于连接 ICM20948
** 入口参数 :
**           tx_data : 数据, 0xFF 如果不需要发送数据 (1 Byte)
**           addr : 寄存器地址(1 Byte)
**           rwflag : 读写标志(1:读，0:写)
**           rx_buf : 接收数据缓存(1 Byte)
** 出口参数 : 无
*******************************************************************************/
void SwapByteSPI2(uint8_t tx_data, uint8_t addr, uint8_t rwflag, uint8_t *rx_buf)
{
  // 【TODO】：使用结构体封装
  // 【TODO】：进行极限测试，减少代码量，提高速率
  uint8_t tx = 0;
  uint8_t rx = 0;
  // 准备时钟线，依据手册先要产生下降沿发送数据
  LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_6);
  // 片选 ICM20948，依据手册要拉低
  LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_7);
  // 计算待访问地址
  tx = rwflag << 7 | addr; // 读写标志位 | 地址
  for (uint8_t i = 0; i < 8; i++)
  {
    // 单片机发送 bit，操作 MOSI，高位优先
    SPI2_MOSI_OUT((tx & 0x80));
    tx <<= 1;
    // 将 SCK 拉低
    LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_6);
    // 等待 ICM20948 发送
    LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_6);
    LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_6);
    // 将 SCK 拉高
    LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_6);
    // 等待 ICM20948 接收
    LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_6);
    LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_6);

    // LL_uDelay(10);
  }

  // 待发送数据
  tx = tx_data;
  for (uint8_t i = 0; i < 8; i++)
  {
    // 将 SCK 拉低
    LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_6);
    // 等待 ICM20948 发送
    LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_6);
    LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_6);
    // 操作 SDI，高位优先：tx_buf & (0x80 >> i)
    SPI2_MOSI_OUT((tx & 0x80));
    tx <<= 1;
    // 读取 SDO
    rx <<= 1;
    rx |= LL_GPIO_IsInputPinSet(GPIOB, LL_GPIO_PIN_4);
    // 将 SCK 拉高
    LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_6);
    // 等待 ICM20948 接收
    LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_6);
    LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_6);
  }
  // 发送完毕，取消片选
  LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_7);
  // 返回数据
  *rx_buf = rx;
  // 【TODO】：状态返回
  // 【TODO】：安全措施
}

/*******************************************************************************
** 名    称 ： ICMGetDate
** 功    能 ： ICM20948 数据读取（g，dps）
** 入口参数 ： 无
** 出口参数 ： 无
*******************************************************************************/
void ICMGetDate(ICM_Data_t *pbuf)
{
  // 【TODO】：进行极限测试，减少代码量，提高速率
  // 【TODO】：量程自适应
  // 0. 获取 ICM20948 帧差，帧号
  pbuf->frame_diff = icm_frame_count - pbuf->frame_count - 1;
  pbuf->frame_count = icm_frame_count;
  // 1. 初始化 ICM20948
  // ICM_Init();
  // 2. 获取原始数据
  ICMOriginalDataTypeDef original_data;
  ICMGetOriginalData(&original_data);
  // 3. 解析 Acc 数据
  pbuf->acc[0] = (int16_t)(original_data.acc_xh << 8) | original_data.acc_xl; // Attention: 使用 int16_t，使符号位溢出，再转化为 float
  pbuf->acc[0] /= 16384.0f;
  pbuf->acc[1] = (int16_t)(original_data.acc_yh << 8) | original_data.acc_yl;
  pbuf->acc[1] /= 16384.0f;
  pbuf->acc[2] = (int16_t)(original_data.acc_zh << 8) | original_data.acc_zl;
  pbuf->acc[2] /= 16384.0f;
  // 4. 解析 Gyro 数据
  pbuf->gyro[0] = (int16_t)(original_data.gyro_xh << 8) | original_data.gyro_xl;
  pbuf->gyro[0] /= 131.0f;
  pbuf->gyro[1] = (int16_t)(original_data.gyro_yh << 8) | original_data.gyro_yl;
  pbuf->gyro[1] /= 131.0f;
  pbuf->gyro[2] = (int16_t)(original_data.gyro_zh << 8) | original_data.gyro_zl;
  pbuf->gyro[2] /= 131.0f;
}


extern void Send32BitToVofa(float);
extern ICM_Data_t icm_data;
/*******************************************************************************
** 名    称 ： ICMIntCallback
** 功    能 ： ICM20948 中断回调函数，当前实现的基本功能为 ICM20948 帧号计数
** 入口参数 ： 无
** 出口参数 ： 无
*******************************************************************************/
__WEAK void ICMIntCallback(void)
{
  // 传感器每发送一帧，触发一次中断，帧计数 +1
  icm_frame_count++;
  // 读取 ICM20948 数据
  ICMGetDate(&icm_data);
}
/********************************END OF FILE***********************************/
